/*
 * @Description: 参数定义
 * @Author: Dryad
 * @Date: 2019-06-14 21:40:15
 */
#include <rtthread.h>
#include <rthw.h>
#include <arm_math.h>
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include "./parameter.h"
#include "./optparse/optparse.h"

#define LOG_TAG "parameter"  /* 该模块TAG为parameter */
#define LOG_LVL LOG_LVL_INFO /* 静态过滤级别为调试级 */
#include <ulog.h>            /* 必须放在宏定义下面 */

#define UPDATE_PARA(para, value, data, size) \
    {                                        \
        para = value;                        \
        data = &para;                        \
        size = sizeof(para);                 \
    }

const struct AGV_Parameter_Struct default_agv_parameter = {
    .agv_address = 1,            /* 从机地址 */
    .obstacle_velocity = 100.0f, /* 有障碍物时的低速度(mm/s) */
    .dec_emergency_time = 1.0f,  /* 急停时的减速度时间(s) */

    .default_acc = 2.0f, /* 默认加速度时间(s)，从零速到最高速所需时间 */
    .default_dec = 2.0f, /* 默认减速度时间(s)，从最高速到零速所需时间 */

    .motor_max_rpm = 3000.0f,       /* 电机最高转速(n/min) */
    .motor_min_rpm = 150.0f,        /* 电机最低转速(n/min) */
    .wheel_diameter = 127.0f,       /* 车轮直径(mm) */
    .motor_reduction_ratio = 30.0f, /* 减速比 */

    .distance_x = 378.0f, /* 两轮之间x轴距离(mm) */
    .distance_y = 400.0f, /* 两轮之间y轴距离(mm) */

    .overall_x = 410.0f, /* 车宽(mm) */
    .overall_y = 700.0f, /* 车长(mm) */

    .pgv_x = 0.0f,            /* PGV传感器x轴偏置(mm) */
    .pgv_y = 0.0f,            /* PGV传感器y轴偏置(mm) */
    .pgv_theta = 0.0f,        /* PGV传感器安装角度(°) */
    .pgv_theta_offset = 0.0f, /* PGV传感器theta偏置(°) */

    .wtd_enable_flag = 0, /* 看门狗使能标志 */
    .avoid_slave = 2,     /* 避障传感器从机地址 */
    .indicate_slave = 2,  /* 运行指示从机地址 */
    .hmi_slave = 1,       /* HMI从机地址 */
};
static struct AGV_Parameter_Struct _parameter;
const struct AGV_Parameter_Struct *const agv_parameter = &_parameter;

static const char *eeprom_start = "Mecanum Robot"; /* 保存在EEPROM开头 */
static rt_device_t dev = RT_NULL;

static void Cal_Parameter(void); /* 根据参数计算一些常量 */

/**
 * @description: 初始化参数
 * @param {type} 
 * @return: 
 */
void Init_Parameter(void)
{
    dev = rt_device_find("fm24cl64"); /* 查找EEPROM */
    if (dev == RT_NULL)
    {
        rt_kprintf("check name of fm24cl64, in %s (%d)\n", __FILE__, __LINE__);
        return;
    }

    rt_device_open(dev, RT_DEVICE_FLAG_RDWR);

    int size_temp = rt_strlen(eeprom_start);           /* 计算起始字符串长度 */
    char *str_temp = (char *)rt_malloc(size_temp + 1); /* 申请内存 */

    rt_device_read(dev, 0, str_temp, size_temp); /* 读取起始字符串 */
    str_temp[size_temp] = '\0';                  /* 以'\0'结尾 */

    if (0 != rt_strcmp(eeprom_start, str_temp))
    {
        /* 两个字符串不相等 */
        rt_free(str_temp);       /* 释放内存 */
        LOG_W("string error");   /* 起始字符串错误 */
        Set_Default_Parameter(); /* 恢复出厂设置 */
        Save_Parameter();        /* 保存参数 */
        return;
    }
    rt_free(str_temp); /* 释放内存 */

    int parameter_size = 0;
    rt_device_read(dev, size_temp, &parameter_size, sizeof(int)); /* 读取参数长度 */
    if (parameter_size != sizeof(struct AGV_Parameter_Struct))
    {
        /* 参数字节数不同 */
        Set_Default_Parameter(); /* 恢复出厂设置 */
        LOG_W("size error");     /* 参数字节数错误 */
        Save_Parameter();        /* 保存参数 */
        return;
    }

    rt_device_read(dev, size_temp + sizeof(parameter_size), &_parameter, sizeof(struct AGV_Parameter_Struct)); /* 读出参数 */
    Cal_Parameter();
}

/**
 * @description: 更新指定参数
 * @param {type} 
 * @return: 
 */
void Updata_Parameter(enum AGV_Parameter_Enum index, const union parameter_data_union value)
{
    int size = 0;
    void *data = RT_NULL;
    switch (index)
    {
    case Parameter_AGV_Address: /* 从机地址 */
        UPDATE_PARA(_parameter.agv_address, value.cvalue, data, size)
        break;
    case Parameter_Obstacle_Velocity: /* 有障碍物时的低速度(mm/s) */
        UPDATE_PARA(_parameter.obstacle_velocity, value.fvalue, data, size)
        break;
    case Parameter_Dec_Emergency_Time: /* 急停时的减速度时间(s) */
        UPDATE_PARA(_parameter.dec_emergency_time, value.fvalue, data, size)
        break;
    case Parameter_Default_Acc: /* 默认加速度时间(s)，从零速到最高速所需时间 */
        UPDATE_PARA(_parameter.default_acc, value.fvalue, data, size)
        break;
    case Parameter_Default_Dec: /* 默认减速度时间(s)，从最高速到零速所需时间 */
        UPDATE_PARA(_parameter.default_dec, value.fvalue, data, size)
        break;
    case Parameter_Motor_Max_RPM: /* 电机最高转速(n/min) */
        UPDATE_PARA(_parameter.motor_max_rpm, value.fvalue, data, size)
        break;
    case Parameter_Moter_Min_RPM: /* 电机最低转速(n/min) */
        UPDATE_PARA(_parameter.motor_min_rpm, value.fvalue, data, size)
        break;
    case Parameter_Wheel_Diameter: /* 车轮直径(mm) */
        UPDATE_PARA(_parameter.wheel_diameter, value.fvalue, data, size)
        break;
    case Parameter_Motor_Reduction_Ratio: /* 电机减速比 */
        UPDATE_PARA(_parameter.motor_reduction_ratio, value.fvalue, data, size)
        break;
    case Parameter_Distance_X: /* 两轮之间x轴距离 */
        UPDATE_PARA(_parameter.distance_x, value.fvalue, data, size)
        break;
    case Parameter_Disyance_Y: /* 两轮之间y轴距离 */
        UPDATE_PARA(_parameter.distance_y, value.fvalue, data, size)
        break;
    case Parameter_Overall_X: /* 车宽 */
        UPDATE_PARA(_parameter.overall_x, value.fvalue, data, size)
        break;
    case Parameter_Overall_Y: /* 车长 */
        UPDATE_PARA(_parameter.overall_y, value.fvalue, data, size)
        break;
    case Parameter_PGV_X: /* PGV传感器x轴偏置(mm) */
        UPDATE_PARA(_parameter.pgv_x, value.fvalue, data, size)
        break;
    case Parameter_PGV_Y: /* PGV传感器y轴偏置(mm) */
        UPDATE_PARA(_parameter.pgv_y, value.fvalue, data, size)
        break;
    case Parameter_PGV_Theta: /* PGV传感器安装角度(°) */
        UPDATE_PARA(_parameter.pgv_theta, value.fvalue, data, size)
        break;
    case Parameter_PGV_Theta_Offset: /* PGV传感器theta偏置(°) */
        UPDATE_PARA(_parameter.pgv_theta_offset, value.fvalue, data, size)
        break;
    case Parameter_WTD_Enable: /* 看门狗使能标志 */
        UPDATE_PARA(_parameter.wtd_enable_flag, value.cvalue, data, size)
        break;
    case Parameter_Avoid_Slave: /* 避障传感器从机地址 */
        UPDATE_PARA(_parameter.avoid_slave, value.cvalue, data, size)
        break;
    case Parameter_Indicate_Slave: /* 运行指示从机地址 */
        UPDATE_PARA(_parameter.indicate_slave, value.cvalue, data, size)
        break;
    case Parameter_HMI_Slave: /* HMI从机地址 */
        UPDATE_PARA(_parameter.hmi_slave, value.cvalue, data, size)
        break;
    default:
        break;
    }
    if (RT_NULL != data)
    {
        /* 修改了参数 */
        int offset = 0;
        offset = (int)data - (int)&_parameter;                              /* 计算参数偏移量 */
        int size_temp = rt_strlen(eeprom_start);                            /* 计算起始字符串长度 */
        rt_device_write(dev, size_temp + sizeof(int) + offset, data, size); /* 读出参数 */
    }
}

/**
 * @description: 恢复出厂设置
 * @param {type} 
 * @return: 
 */
void Set_Default_Parameter(void)
{
    rt_enter_critical();
    rt_memcpy(&_parameter, &default_agv_parameter, sizeof(struct AGV_Parameter_Struct));
    rt_exit_critical();
    rt_kprintf("parameter reste\n");
    Cal_Parameter();
}

/**
 * @description: 保存参数
 * @param {type} 
 * @return: 
 */
void Save_Parameter(void)
{
    int size_temp = rt_strlen(eeprom_start);                                               /* 计算起始字符串长度 */
    rt_device_write(dev, 0, eeprom_start, size_temp);                                      /* 保存起始字符串 */
    int parameter_size = sizeof(struct AGV_Parameter_Struct);                              /* 计算参数长度 */
    rt_device_write(dev, size_temp, &parameter_size, sizeof(parameter_size));              /* 保存参数长度 */
    rt_device_write(dev, size_temp + sizeof(parameter_size), &_parameter, parameter_size); /* 保存参数 */
}

void Cal_Parameter(void)
{
    float temp = 0.0f;

    temp = _parameter.motor_max_rpm / _parameter.motor_reduction_ratio / 60 * PI * _parameter.wheel_diameter;
    _parameter.max_velocity_line = temp; /* 最大线速度(mm/s) */

    temp = _parameter.motor_min_rpm / _parameter.motor_reduction_ratio / 60 * PI * _parameter.wheel_diameter;
    _parameter.min_velocity_line = temp; /* 最小线速度(mm/s) */

    temp = _parameter.max_velocity_line / ((_parameter.distance_x + _parameter.distance_y) / 2.0f) / PI * 180.0f;
    _parameter.max_angular_velocity = temp; /* 最大角速度(°/s) */

    temp = _parameter.min_velocity_line / ((_parameter.distance_x + _parameter.distance_y) / 2.0f) / PI * 180.0f;
    _parameter.min_angular_velocity = temp; /* 最小角速度(°/s) */

    temp = atanf(_parameter.overall_x / _parameter.overall_y) * 180 / PI;
    _parameter.agv_overall_offset_angle = temp; /* 车外形的偏置角(°) */

    _parameter.distance_lx_ly = (_parameter.distance_x + _parameter.distance_y) / 2.0f;
    _parameter.acc_vx = _parameter.max_velocity_line / _parameter.default_acc;
    _parameter.acc_vy = _parameter.acc_vx;
    _parameter.acc_w = _parameter.max_angular_velocity / _parameter.default_acc;
}

static void Parameter_Control_Help(void)
{
    rt_kprintf("\n");
    rt_kprintf("parameter [-h --help] [--save] [--reset] \n");
    rt_kprintf("parameter [-r --read] (<para index>) \n");
    rt_kprintf("parameter [-w --write] (<para index>) (<-v --value>) (<value>) \n");
    rt_kprintf("optional arguments:\n");
    rt_kprintf("  -h, --help      show this help message and exit\n");
    rt_kprintf("  --save          save the parameter\n");
    rt_kprintf("  --reset         reset the parameter\n");
    rt_kprintf("  -r, --read      read parameter\n");
    rt_kprintf("  -w, --write     write parameter\n");
    rt_kprintf("  -v, --value     parameter value\n");
}

static struct optparse_long long_opts[] = {
    {"help", 'h', OPTPARSE_NONE},
    {"reset", 0, OPTPARSE_NONE},
    {"save", 0, OPTPARSE_NONE},
    {"write", 'w', OPTPARSE_REQUIRED},
    {"read", 'r', OPTPARSE_REQUIRED},
    {"value", 'v', OPTPARSE_REQUIRED},
    {"index", 'i', OPTPARSE_REQUIRED},
    {RT_NULL, 0, OPTPARSE_NONE},
};

static rt_err_t parameter(int argc, char **argv)
{
    int opt;
    int cmd_index;
    struct optparse options;

    if (argc == 1)
    {
        Parameter_Control_Help();
        return RT_EOK;
    }

    optparse_init(&options, argv);

    enum AGV_Parameter_Enum para_index = Parameter_AGV_Address;
    char write_value_flag = 0, read_value_flag = 0, set_value_flag = 0;
    union parameter_data_union value;

    while ((opt = optparse_long(&options, long_opts, &cmd_index)) != -1)
    {
        switch (cmd_index)
        {
        case 0: /* help指令 */
            Parameter_Control_Help();
            break;
        case 1: /* reset指令 */
            Set_Default_Parameter();
        case 2: /* save指令 */
            Save_Parameter();
            break;
        case 3: /* write指令 */
            para_index = (enum AGV_Parameter_Enum)atoi(options.optarg);
            write_value_flag = 1;
            break;
        case 4: /* read指令 */
            para_index = (enum AGV_Parameter_Enum)atoi(options.optarg);
            read_value_flag = 1;
            break;
        case 5: /* value指令 */
        {
            switch (para_index)
            {
            case Parameter_AGV_Address:
            case Parameter_WTD_Enable:
            case Parameter_Avoid_Slave:
            case Parameter_Indicate_Slave:
            case Parameter_HMI_Slave:
                value.cvalue = atoi(options.optarg);
                break;
            default:
                value.fvalue = atof(options.optarg);
                break;
            }
        }
            set_value_flag = 1;
            break;
        default:
            if ('?' == opt)
            {
                Parameter_Control_Help();
                rt_kprintf("%s: %s\n", argv[0], options.errmsg);
                return -RT_ERROR;
            }
            break;
        }
    }

    if (write_value_flag && set_value_flag)
    {
        Updata_Parameter(para_index, value);
        read_value_flag = 1;
    }

    char str[80];
    int str_size = 0;
    if (read_value_flag)
    {
        switch (para_index)
        {
        case Parameter_AGV_Address: /* 从机地址 */
            str_size = sprintf(str, "agv address: %d\n", (int)_parameter.agv_address);
            break;
        case Parameter_Obstacle_Velocity: /* 有障碍物时的低速度(mm/s) */
            str_size = sprintf(str, "obstacle velocity: %.2f\n", _parameter.obstacle_velocity);
            break;
        case Parameter_Dec_Emergency_Time: /* 急停时的减速度时间(s) */
            str_size = sprintf(str, "dec emergency time: %.2f\n", _parameter.dec_emergency_time);
            break;
        case Parameter_Default_Acc: /* 默认加速度时间(s)，从零速到最高速所需时间 */
            str_size = sprintf(str, "default acc time: %.2f\n", _parameter.default_acc);
            break;
        case Parameter_Default_Dec: /* 默认减速度时间(s)，从最高速到零速所需时间 */
            str_size = sprintf(str, "default dec time: %.2f\n", _parameter.default_dec);
            break;
        case Parameter_Motor_Max_RPM: /* 电机最高转速(n/min) */
            str_size = sprintf(str, "motor max rpm: %.2f\n", _parameter.motor_max_rpm);
            break;
        case Parameter_Moter_Min_RPM: /* 电机最低转速(n/min) */
            str_size = sprintf(str, "motor min rpm: %.2f\n", _parameter.motor_min_rpm);
            break;
        case Parameter_Wheel_Diameter: /* 车轮直径(mm) */
            str_size = sprintf(str, "wheel diameter: %.2f\n", _parameter.wheel_diameter);
            break;
        case Parameter_Motor_Reduction_Ratio: /* 电机减速比 */
            str_size = sprintf(str, "motor reduction ratio: %.2f\n", _parameter.motor_reduction_ratio);
            break;
        case Parameter_Distance_X: /* 两轮之间x轴距离 */
            str_size = sprintf(str, "distance x: %.2f\n", _parameter.distance_x);
            break;
        case Parameter_Disyance_Y: /* 两轮之间y轴距离 */
            str_size = sprintf(str, "distance y: %.2f\n", _parameter.distance_y);
            break;
        case Parameter_Overall_X: /* 车宽 */
            str_size = sprintf(str, "overall x: %.2f\n", _parameter.overall_x);
            break;
        case Parameter_Overall_Y: /* 车长 */
            str_size = sprintf(str, "overall y: %.2f\n", _parameter.overall_y);
            break;
        case Parameter_Max_Velocity: /* 最大线速度(mm/s) */
            str_size = sprintf(str, "max velocity line: %.2f\n", _parameter.max_velocity_line);
            break;
        case Parameter_Min_Velocity: /* 最小线速度(mm/s) */
            str_size = sprintf(str, "min velocity line: %.2f\n", _parameter.min_velocity_line);
            break;
        case Parameter_Max_Angular_Velocity: /* 最大角速度(°/s) */
            str_size = sprintf(str, "max angular velocity: %.2f\n", _parameter.max_angular_velocity);
            break;
        case Parameter_Min_Angular_Velocity: /* 最大角速度(°/s) */
            str_size = sprintf(str, "min angular velocity: %.2f\n", _parameter.min_angular_velocity);
            break;
        case Parameter_PGV_X: /* PGV传感器x轴偏置(mm) */
            str_size = sprintf(str, "pgv x coor: %.2f\n", _parameter.pgv_x);
            break;
        case Parameter_PGV_Y: /* PGV传感器y轴偏置(mm) */
            str_size = sprintf(str, "pgv y coor: %.2f\n", _parameter.pgv_y);
            break;
        case Parameter_PGV_Theta: /* PGV传感器安装角度(°) */
            str_size = sprintf(str, "pgv theta coor: %.2f\n", _parameter.pgv_theta);
            break;
        case Parameter_PGV_Theta_Offset: /* PGV传感器theta偏置(°) */
            str_size = sprintf(str, "pgv theta coor offset: %.2f\n", _parameter.pgv_theta_offset);
            break;
        case Parameter_WTD_Enable: /* 看门狗使能标志 */
            str_size = sprintf(str, "watchdog %s\n", (_parameter.wtd_enable_flag ? "on" : "off"));
            break;
        case Parameter_Avoid_Slave: /* 避障传感器从机地址 */
            str_size = sprintf(str, "avoid sensor addr: %d\n", _parameter.avoid_slave);
            break;
        case Parameter_Indicate_Slave: /* 运行指示从机地址 */
            str_size = sprintf(str, "indicate addr: %d\n", _parameter.indicate_slave);
            break;
        case Parameter_HMI_Slave: /* HMI从机地址 */
            str_size = sprintf(str, "hmi addr: %d\n", _parameter.hmi_slave);
            break;
        default:
            break;
        }
        if (str_size)
        {
            rt_kprintf(str);
        }
    }

    return RT_EOK;
}
MSH_CMD_EXPORT(parameter, parameter);
